#include "motion9_dll.h"
#include "platform.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
FILE *inter_out;
FILE *test_fd;
double current_position[MAX_JOINT];

//消息回调
int MsgCallback(int reason, char *msg) {
    if (reason < 2000)
        printf("reason=%d\n", reason);
    else if (reason < 3000)
        printf("reason=%d,msg:%s", reason, msg);
    else{
        printf("reason=%d msg=%s\n",reason, msg);
    }
    return 0;
}
//位置输出回调
int OutCallback(double posit[MAX_JOINT], float vel[MAX_JOINT], float acc[MAX_JOINT]) {
    static double add_time = 0;
    add_time += 0.002;
    memcpy(current_position, posit, sizeof(double) * MAX_JOINT);
#if 0
    fprintf(inter_out, "%.6f %.6f\n", add_time, vel[0]);
    //STRUCT_POSE pose;
    //Motion_KmClampForward(posit, &pose, NULL);
    //fprintf(inter_out,"%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n",
    //	//posit[0],posit[1],posit[2],posit[3],
    //	//posit[4],posit[5],posit[6],posit[7],posit[8]
    //	vel[0], vel[1], vel[2], vel[3],
    //	pose.x,pose.y,pose.z,pose.rz,0.0
    //	);
    fflush(inter_out);
#endif

#if 0
    fprintf(stdout, "%.6f %.6f %.6f %.6f %.3f %.3f %.3f %.3f %.3f %.3f\n",
            posit[0], posit[1], posit[2], posit[3],
            //posit[4],posit[5],posit[6],posit[7],posit[8]
            vel[0], vel[1], vel[2], vel[3], 0.0, 0.0
            //pose.x,pose.y,pose.z,pose.rz,0.0
            );
//fprintf(inter_out, "%.6f %.6f %.6f %.6f %.3f %.3f %.3f %.3f %.3f %.3f\n",
//	posit[0], posit[1], posit[2], posit[3],
//	//posit[4],posit[5],posit[6],posit[7],posit[8]
//	vel[0], vel[1], vel[2], vel[3],0.0
//	//pose.x,pose.y,pose.z,pose.rz,0.0
//	);
#endif

#if 0
	DWORD counter = timeGetTime();
	printf("%d\n", counter);
#endif
    return 0;
}

//Motion计算线程
void *Serv(void *param) {
    while (1) {
        C_SLEEP_MS(2);
        Motion_Service();
    }
    pthread_exit(0);
}

//初始化
int initial_motion() {
    STRUCT_MOTION_PARM para = {0};
    //结构和速度配置
    float max_vel[MAX_JOINT] = {3.8765, 6.1569, 500, 6.1569, 10, 10, 10, 10, 10};
    float max_acc[MAX_JOINT] = {3.8765 * 5, 6.1569 * 5, 500 * 5, 6.1569 * 5, 100, 100, 100, 100, 100};
    double abs_limit_h[MAX_JOINT] = {2.8, 2.5, 150, 6.28, 0, 0, 0, 0, 0};
    double abs_limit_l[MAX_JOINT] = {-2.8, -2.5, -150, -6.28, 0, 0, 0, 0, 0};
    double soft_limit_h[MAX_JOINT] = {2.5, 2.0, 120, 6.0, 0, 0, 0, 0, 0};
    double soft_limit_l[MAX_JOINT] = {-2.5, -2.0, -120, -6.0, 0, 0, 0, 0, 0};
    para.machine_struct = SCARA;
    para.dh_machine[0].a = 250;
    para.dh_machine[0].d = 0;
    para.dh_machine[1].a = 150;
    para.callback_msg = MsgCallback;
    para.callback_out = OutCallback;
    memcpy(para.vel_joint.vel_max, max_vel, sizeof(float) * MAX_JOINT);
    memcpy(para.vel_joint.acc_max, max_acc, sizeof(float) * MAX_JOINT);
    para.vel_line.vel_max = 2000;
    para.vel_line.acc_max = 50000;
    para.vel_rot.vel_max = 10;
    para.vel_rot.acc_max = 10;
    //位置限制配置
    memcpy(para.limit_abs.limit_h, abs_limit_h, sizeof(double) * MAX_JOINT);
    memcpy(para.limit_abs.limit_l, abs_limit_l, sizeof(double) * MAX_JOINT);
    memcpy(para.limit_soft.limit_h, abs_limit_h, sizeof(double) * MAX_JOINT);
    memcpy(para.limit_soft.limit_l, abs_limit_l, sizeof(double) * MAX_JOINT);
    para.inter_period = (float) 0.02;
    para.joint_period = (float) 0.002;
    para.fd1 = test_fd;
    Motion_Init(para);
    return 0;
}

//
int test_free() {
    STRUCT_USER_VEL vel;
    vel.jv.vel = 1.0;
    vel.jv.acca = 0.1;
    vel.jv.accd = 0.1;
    vel.lv.vel = 500;
    vel.lv.acca = 10000;
    vel.lv.accd = 10000;
    vel.rv.vel = 0.5;
    vel.rv.acca = 5;
    vel.rv.accd = 5;
    STRUCT_POSE pose1 = {0};
    pose1.x = 200;
    pose1.y = -100;
    STRUCT_POSE pose2 = {0};
    pose2.x = 200;
    pose2.y = 100;
    STRUCT_POSE pose3 = {0};
    pose3.x = 300;
    pose3.y = 100;
    STRUCT_POSE pose4 = {0};
    pose4.x = 300;
    pose4.y = -100;
    double posit[9] = {2.2, 0, 0, 0, 0, 0, 0, 0, 0};
    double posit1[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
#if 1
    for (int i = 0; i < 1; i++) {
        Motion_Add_Joint_Free_Jog_Abstract_Linked(1, posit, vel, 0, 1);
        Motion_WaitMoveDone(10, 100000);
        Motion_Add_Joint_Free_Jog_Abstract_Linked(1, posit1, vel, 0, 1);
        Motion_WaitMoveDone(10, 100000);
    }
#endif
#if 0
	for (int i = 0; i < 1; i++){
		Motion_Add_Axis_Free_Jog_Abstract(1, pose1, 0, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
		//Motion_Add_Axis_Free_Jog_Abstract(1, pose2, 1, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
		//Motion_Add_Axis_Free_Jog_Abstract(1, pose3, 0, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
		//Motion_Add_Axis_Free_Jog_Abstract(1, pose4, 0, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
	}
#endif
    return 0;
}

int test_plane_p2p_posit() {
    STRUCT_USER_VEL vel = {
        {0.8, 1.0, 1.0},
        {500, 10000, 10000},
        {0.5, 5, 5}};
    STRUCT_USER_OVL ovl = {1, 0, 0};
    int id = 0;
    double posit1[MAX_JOINT] = {0.5, 0.5, 0.0, 0.0};
    double posit2[MAX_JOINT] = {2.0, 1.0, 50.0, 0.0};
    double posit3[MAX_JOINT] = {1.0, 1.0, 0.0, 0.0};
    double posit4[MAX_JOINT] = {-1.0, -1.0, 0.0, 0.0};
    double posit5[MAX_JOINT] = {-1.0, -1.0, 50.0, 0.0};
    double posit6[MAX_JOINT] = {-1.0, -1.0, 0.0, 0.0};
    for (int i = 0; i < 1000000; i++) {
        Motion_AddPlaneP2P_Posit(posit1, vel, ovl, ++id, 1);
        Motion_WaitMoveDone(10, 100000);
        Motion_AddPlaneP2P_Posit(posit2, vel, ovl, ++id, 1);
        Motion_WaitMoveDone(10, 100000);
        Motion_AddPlaneP2P_Posit(posit3, vel, ovl, ++id, 1);
        Motion_WaitMoveDone(10, 100000);
        Motion_AddPlaneP2P_Posit(posit4, vel, ovl, ++id, 1);
        Motion_WaitMoveDone(10, 100000);
        Motion_AddPlaneP2P_Posit(posit5, vel, ovl, ++id, 1);
        Motion_WaitMoveDone(10, 100000);
        Motion_AddPlaneP2P_Posit(posit6, vel, ovl, ++id, 1);
        Motion_WaitMoveDone(10, 100000);
        printf("--------i=%d------------\n",i);
    }
    return 0;
}

int test_plane_p2p_pose() {
    STRUCT_USER_VEL vel = {
        {1.0, 1.0, 1.0},
        {500, 10000, 10000},
        {0.5, 5, 5}};
    STRUCT_USER_OVL ovl = {1, 0, 0};
    int id = 0;
    STRUCT_POSE pose1 = {200, -200, 0, 0, 0, 0};
    STRUCT_POSE pose2 = {200, 200, 0, 0, 0, 0};
    STRUCT_POSE pose3 = {300, 200, 0, 0, 0, 0};
    STRUCT_POSE pose4 = {300, -200, 0, 0, 0, 0};
    STRUCT_POSE pose5 = {300, -200, 0, 0, 0, 0};
    STRUCT_POSE pose6 = {200, -200, 0, 0, 0, 0};
    for (int i = 0; i < 100; i++) {
        Motion_AddPlaneP2P_Pose(pose1, 0, vel, ovl, ++id, 1);
        Motion_AddPlaneP2P_Pose(pose2, 0, vel, ovl, ++id, 1);
        Motion_AddPlaneP2P_Pose(pose3, 0, vel, ovl, ++id, 1);
        Motion_AddPlaneP2P_Pose(pose4, 0, vel, ovl, ++id, 1);
        Motion_AddPlaneP2P_Pose(pose5, 0, vel, ovl, ++id, 1);
        //int ttt = rand() % 4000;
        //printf("t=%d\n", ttt);
        //Sleep(ttt);
        //Motion_CommandPause();
        //Sleep(rand() % 4000);
        //Motion_CommandResume();
        Motion_WaitMoveDone(10, 100000);
    }
    return 0;
}

int test_fkine() {
    double posit[MAX_JOINT] = {-0.4, 1, 0, 0.5, 0, 0, 0, 0, 0};
    STRUCT_POSE pose_curr;
    STRUCT_POSE pose_t = {65, 0, 0, 0, 0, 0};
    Motion_SetTool(&pose_t);
    Motion_KmClampForward(posit, &pose_curr, NULL);
    STRUCT_POSE pose_2 = {300, -78.09, 80.000000, 0, 0, 1.570796};
    double inv_j[MAX_JOINT];
    int ret = Motion_KmClampInverse(inv_j, &pose_2, 1);
    return 0;
}

int test_plane_line_pose() {
    STRUCT_USER_VEL vel = {
        {1.0, 1.0, 1.0, 0},
        {500, 1000, 1000, 0},
        {0.5, 5, 5, 0}};
    STRUCT_USER_OVL ovl = {1, 0, 0};
    int id = 0;
    STRUCT_POSE pose1 = {200, -200, 0, 0, 0, 0};
    STRUCT_POSE pose2 = {200, 200, 0, 0, 0, 0};
    STRUCT_POSE pose3 = {300, 200, 0, 0, 0, 0};
    STRUCT_POSE pose4 = {300, -200, 0, 0, 0, 0};
    STRUCT_POSE pose5 = {300, -200, 0, 0, 0, 0};
    STRUCT_POSE pose6 = {200, -200, 0, 0, 0, 0};
    Motion_AddPlaneP2P_Pose(pose1, 0, vel, ovl, ++id, 1);
    for (int i = 0; i < 400; i++) {
        Motion_AddPlaneLine_Pose(pose2, 0, vel, ovl, ++id, 1);
        Motion_AddPlaneLine_Pose(pose3, 0, vel, ovl, ++id, 1);
        Motion_AddPlaneLine_Pose(pose4, 0, vel, ovl, ++id, 1);
        Motion_AddPlaneLine_Pose(pose1, 0, vel, ovl, ++id, 1);
        //int ttt = rand() % 4000;
        //printf("t=%d\n", ttt);
        //Sleep(ttt);
        //Motion_CommandPause();
        //Sleep(rand() % 4000);
        //Motion_CommandResume();
        Motion_WaitMoveDone(10, 100000);
    }
    return 0;
}

int main() {
    int ret = 0;
    pthread_t thread_id;
    double posit[MAX_JOINT] = {0, 0, 0, 0};
    inter_out = fopen("out.txt", "w+");
    test_fd = fopen("test.txt", "w+");
    initial_motion();
    test_fkine();
    pthread_create(&thread_id, NULL, Serv, NULL);
    Motion_SetCurrentPosition(posit);
    Motion_CommandEmergencyRelease();
    Motion_CommandPlane();
    test_plane_p2p_posit();
    while (1) {
        sleep_ms(1000);
    }
    return 0;
}
